#include "Quaternion.h"

using namespace XYF;

Quaternion::Quaternion(Real s, Vector3f v) : m_s(s), m_v(v)
{
}

Quaternion::Quaternion(Vector3f v, Real theta) : m_s(cos(theta / 2)), m_v(normalize(v) * sin(theta / 2))
{
}

Quaternion::Quaternion(Vector3f a, Vector3f b)
{
    // 确保两个轴长度不为零
    assert(length(a) > FLT_MIN && length(b) > FLT_MIN);

    // 先把 a 转动到 z 轴：先绕 z 轴转，然后绕 y 轴转
    Quaternion q1(1, {0, 0, 0}), q2(1, {0, 0, 0});
    double proj = sqrt(pow(a.x(), 2) + pow(a.y(), 2));

    // 如果已经在 z 轴，就不动；否则计算两个四元数
    if (proj > FLT_MIN)
    {
        q1 = Quaternion({0, 0, 1}, -acos(a.x() / proj));
        q2 = Quaternion({0, 1, 0}, -acos(a.z() / length(a)));
    }

    // 然后把 b 同样转动到 z 轴
    Quaternion q3(1, {0, 0, 0}), q4(1, {0, 0, 0});
    proj = sqrt(pow(b.x(), 2) + pow(b.y(), 2));

    // 如果已经在 z 轴，就不动；否则计算两个四元数
    if (proj > FLT_MIN)
    {
        q3 = Quaternion({0, 0, 1}, -acos(b.x() / proj));
        q4 = Quaternion({0, 1, 0}, -acos(b.z() / length(b)));
    }

    // 最后初始化
    *this = Quaternion(1, {0, 0, 0}) / q3 / q4 * q2 * q1;
}

Quaternion Quaternion::operator*(Real t) const
{
    return Quaternion(m_s * t, m_v * t);
}

Quaternion Quaternion::operator/(Real t) const
{
    assert(fabs(t) > FLT_MIN);
    return Quaternion(m_s / t, m_v / t);
}

Quaternion Quaternion::operator*(const Quaternion &q) const
{
    return Quaternion(m_s * q.s() - dot(m_v, q.v()), q.v() * m_s + m_v * q.s() + cross(m_v, q.v()));
}

Quaternion Quaternion::operator/(const Quaternion &q) const
{
    return (*this) * Quaternion(q.s(), q.v() * -1) / pow(sqrt(q.s() * q.s() + pow(length(q.v()), 2)), 2);
}

bool Quaternion::operator==(const Quaternion &q) const
{
    if (length(q - *this) < 1e-6)
        return true;
    return false;
}

Quaternion Quaternion::operator+(const Quaternion &q) const
{
    return Quaternion(m_s + q.s(), m_v + q.v());
}

Quaternion Quaternion::operator-(const Quaternion &q) const
{
    return Quaternion(m_s - q.s(), m_v - q.v());
}

Real Quaternion::s() const
{
    return m_s;
}

Vector3f Quaternion::v() const
{
    return m_v;
}

Real &Quaternion::rs()
{
    return m_s;
}

Vector3f &Quaternion::rv()
{
    return m_v;
}

Real XYF::length(Quaternion q)
{
    return sqrt(q.s() * q.s() + pow(length(q.v()), 2));
}

Quaternion XYF::normalize(Quaternion q)
{
    Real len = length(q);
    assert(len > FLT_MIN);
    return q / len;
}
